#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Muto Robot ROS2 Controller Node with FastAPI HTTP Interface

This node provides HTTP endpoints to receive command sequences from remote AI models
and execute them using the muto_controller library.
"""

import threading
import asyncio

import rclpy
from rclpy.node import Node
from std_msgs.msg import String

from fastapi import FastAPI
import uvicorn

from .command_executor import MutoCommandExecutor
from .routes import MutoAPIRoutes
from .camera_manager import CameraNodeManager


class MutoControllerNode(Node):
    """Muto机器人控制节点"""
    
    def __init__(self):
        try:
            super().__init__('muto_controller_node')
            
            # 错误恢复状态跟踪
            self.error_count = 0
            self.max_errors = 10
            self.last_error_time = 0
            self.error_reset_interval = 300  # 5分钟后重置错误计数
            self.component_health = {
                'command_executor': False,
                'fastapi_app': False,
                'api_routes': False,
                'ros2_publisher': False,
                'camera_system': False
            }
            
            # 初始化日志
            self.get_logger().info('Initializing Muto Controller Node...')
            
            # 创建命令执行器
            try:
                # 初始化摄像头节点管理器
                self.camera_manager = CameraNodeManager(logger=self.get_logger())
                
                # 创建命令执行器并传递camera_manager
                self.command_executor = MutoCommandExecutor(logger=self.get_logger(), camera_manager=self.camera_manager)
                self.component_health['command_executor'] = True
                self.get_logger().info('Command executor initialized successfully')
                
                # 预启动摄像头系统
                self._prestart_camera_system()
                
            except Exception as e:
                self.get_logger().error(f'Failed to initialize command executor: {e}')
                # 创建一个基本的命令执行器以保持节点运行
                self.command_executor = None
                self.component_health['command_executor'] = False
            
            # 创建FastAPI应用
            try:
                self.app = FastAPI(
                    title="Muto Robot Controller API",
                    description="HTTP API for controlling Muto hexapod robot",
                    version="1.0.0"
                )
                self.component_health['fastapi_app'] = True
                self.get_logger().info('FastAPI application created successfully')
            except Exception as e:
                self.get_logger().error(f'Failed to create FastAPI application: {e}')
                self.component_health['fastapi_app'] = False
                raise
            
            # 注册API路由
            try:
                self.routes = MutoAPIRoutes(self.app, self.command_executor, logger=self.get_logger())
                self.component_health['api_routes'] = True
                self.get_logger().info('API routes registered successfully')
                try:
                    if self.routes and hasattr(self.routes, 'voice_assistant_service') and self.routes.voice_assistant_service:
                        started = self.routes.voice_assistant_service.start()
                        self.get_logger().info(f'Voice assistant auto-start: {started}')
                except Exception as e:
                    self.get_logger().warn(f'Failed to auto-start voice assistant: {e}')
            except Exception as e:
                self.get_logger().error(f'Failed to register API routes: {e}')
                # 继续运行，但API功能可能受限
                self.routes = None
                self.component_health['api_routes'] = False
            
            # ROS2发布器和订阅器
            try:
                self.status_publisher = self.create_publisher(
                    String, 
                    'muto_controller_status', 
                    10
                )
                self.component_health['ros2_publisher'] = True
                self.get_logger().info('ROS2 publisher created successfully')
            except Exception as e:
                self.get_logger().error(f'Failed to create ROS2 publisher: {e}')
                self.status_publisher = None
                self.component_health['ros2_publisher'] = False
            
            # 状态定时器和健康检查
            try:
                self.create_timer(1.0, self.publish_status)
                self.create_timer(300.0, self.health_check)  # 每5分钟进行健康检查（从30秒改为300秒）
                self.create_timer(600.0, self.attempt_recovery)  # 每10分钟尝试恢复失败组件（从60秒改为600秒）
                self.create_timer(5.0, self.monitor_camera_node)
                self.get_logger().info('Status timer and health check created successfully')
            except Exception as e:
                self.get_logger().error(f'Failed to create timers: {e}')
            
            # FastAPI服务器配置
            self.server_host = "0.0.0.0"
            self.server_port = 8080
            
            self.get_logger().info('Muto Controller Node initialized successfully')
            
        except Exception as e:
            # 如果节点初始化完全失败，记录错误并重新抛出
            if hasattr(self, 'get_logger'):
                self.get_logger().error(f'Critical error during node initialization: {e}')
            else:
                print(f'Critical error during node initialization: {e}')
            raise
    
    def _prestart_camera_system(self):
        """预启动摄像头系统，避免have_a_look时的频繁启停"""
        try:
            if self.command_executor and self.command_executor.muto_interface:
                self.get_logger().info('Attempting to prestart camera system...')
                
                # 首先启动底层摄像头服务
                try:
                    success, message, data = self.command_executor.muto_interface.start_camera(
                        check_before_start=True, 
                        wait_time_s=3.0
                    )
                    
                    if success:
                        self.get_logger().info(f'Camera service started successfully: {message}')
                        
                        # 初始化持久化摄像头节点
                        node_success, node_message = self.camera_manager.initialize_camera_node()
                        if node_success:
                            self.get_logger().info(f'Persistent camera node initialized: {node_message}')
                            self.component_health['camera_system'] = True
                        else:
                            self.get_logger().warn(f'Camera node initialization failed: {node_message}')
                            self.component_health['camera_system'] = False
                    else:
                        self.get_logger().warn(f'Camera service start failed: {message}')
                        self.component_health['camera_system'] = False
                        
                except Exception as camera_error:
                    self.get_logger().warn(f'Camera prestart error (non-critical): {camera_error}')
                    self.component_health['camera_system'] = False
            else:
                self.get_logger().warn('Cannot prestart camera: muto_interface not available')
                self.component_health['camera_system'] = False
                
        except Exception as e:
            self.get_logger().error(f'Error in camera prestart: {e}')
            self.component_health['camera_system'] = False

    def publish_status(self):
        """发布节点状态到ROS2话题"""
        try:
            if self.status_publisher is not None:
                status_msg = String()
                healthy_components = sum(self.component_health.values())
                total_components = len(self.component_health)
                status_msg.data = f"Muto Controller Node Active - Server: {self.server_host}:{self.server_port} - Health: {healthy_components}/{total_components} - Errors: {self.error_count}"
                self.status_publisher.publish(status_msg)
        except Exception as e:
            # 记录错误但不中断程序运行
            self._handle_error(e, 'publish_status')
    
    def health_check(self):
        """执行系统健康检查"""
        try:
            # 检查命令执行器
            if self.command_executor is None:
                self.component_health['command_executor'] = False
            else:
                # 可以添加更详细的健康检查逻辑
                self.component_health['command_executor'] = True
            
            # 检查FastAPI应用
            if self.app is None:
                self.component_health['fastapi_app'] = False
            else:
                self.component_health['fastapi_app'] = True
            
            # 检查API路由
            if self.routes is None:
                self.component_health['api_routes'] = False
            else:
                self.component_health['api_routes'] = True
            
            # 检查ROS2发布器
            if self.status_publisher is None:
                self.component_health['ros2_publisher'] = False
            else:
                self.component_health['ros2_publisher'] = True
            
            # 检查摄像头系统状态 - 优化检查逻辑，减少频繁检查
            if hasattr(self, 'camera_manager') and self.camera_manager:
                try:
                    # 只有在摄像头系统之前是健康状态时才进行详细检查
                    # 如果之前就是不健康状态，跳过检查以减少系统负载
                    previous_camera_health = self.component_health.get('camera_system', False)
                    
                    if previous_camera_health:
                        # 摄像头之前是健康的，进行轻量级检查
                        camera_healthy, camera_msg, camera_data = self.camera_manager.check_camera_node_health()
                        self.component_health['camera_system'] = camera_healthy
                        if not camera_healthy:
                            self.get_logger().info(f'Camera system became unhealthy: {camera_msg}')
                    else:
                        # 摄像头之前就不健康，跳过检查，保持不健康状态
                        # 这样可以避免频繁的无效检查
                        self.component_health['camera_system'] = False
                        self.get_logger().debug('Skipping camera health check - system was already unhealthy')
                        
                except Exception as e:
                    self.get_logger().debug(f'Camera health check error: {e}')
                    self.component_health['camera_system'] = False
            else:
                self.component_health['camera_system'] = False
            
            # 记录健康状态 - 优化日志输出频率
            unhealthy_components = [name for name, healthy in self.component_health.items() if not healthy]
            if unhealthy_components:
                # 只有当不健康组件发生变化时才输出警告日志
                if not hasattr(self, '_last_unhealthy_components'):
                    self._last_unhealthy_components = set()
                
                current_unhealthy_set = set(unhealthy_components)
                if current_unhealthy_set != self._last_unhealthy_components:
                    self.get_logger().warn(f'Unhealthy components detected: {unhealthy_components}')
                    self._last_unhealthy_components = current_unhealthy_set
                else:
                    # 组件状态没有变化，使用debug级别日志
                    self.get_logger().debug(f'Unhealthy components (no change): {unhealthy_components}')
            else:
                # 重置记录的不健康组件
                if hasattr(self, '_last_unhealthy_components'):
                    if self._last_unhealthy_components:
                        self.get_logger().info('All components are now healthy')
                    self._last_unhealthy_components = set()
                self.get_logger().debug('All components are healthy')
                
        except Exception as e:
            self._handle_error(e, 'health_check')
    
    def attempt_recovery(self):
        """尝试恢复失败的组件"""
        try:
            import time
            current_time = time.time()
            
            # 重置错误计数（如果超过重置间隔）
            if current_time - self.last_error_time > self.error_reset_interval:
                self.error_count = 0
            
            # 如果错误太多，跳过恢复尝试
            if self.error_count >= self.max_errors:
                self.get_logger().warn(f'Too many errors ({self.error_count}), skipping recovery attempt')
                return
            
            # 尝试恢复命令执行器
            if not self.component_health['command_executor'] and self.command_executor is None:
                try:
                    self.command_executor = MutoCommandExecutor(logger=self.get_logger())
                    self.component_health['command_executor'] = True
                    self.get_logger().info('Command executor recovered successfully')
                except Exception as e:
                    self.get_logger().warn(f'Failed to recover command executor: {e}')
            
            # 尝试恢复ROS2发布器
            if not self.component_health['ros2_publisher'] and self.status_publisher is None:
                try:
                    self.status_publisher = self.create_publisher(
                        String, 
                        'muto_controller_status', 
                        10
                    )
                    self.component_health['ros2_publisher'] = True
                    self.get_logger().info('ROS2 publisher recovered successfully')
                except Exception as e:
                    self.get_logger().warn(f'Failed to recover ROS2 publisher: {e}')
            
            # 尝试恢复API路由
            if not self.component_health['api_routes'] and self.routes is None and self.app is not None:
                try:
                    self.routes = MutoAPIRoutes(self.app, self.command_executor, logger=self.get_logger())
                    self.component_health['api_routes'] = True
                    self.get_logger().info('API routes recovered successfully')
                except Exception as e:
                    self.get_logger().warn(f'Failed to recover API routes: {e}')
                    
            # 尝试恢复摄像头系统
            if not self.component_health.get('camera_system', False):
                try:
                    self._prestart_camera_system()
                except Exception as e:
                    self.get_logger().error(f'Failed to recover camera system: {e}')
            
        except Exception as e:
            self._handle_error(e, 'attempt_recovery')

    def monitor_camera_node(self):
        try:
            if not (self.command_executor and self.command_executor.muto_interface):
                return
            success, msg, running = self.command_executor.muto_interface.check_ros2_node_running("camera", "/camera")
            if success and running:
                return
            if getattr(self, "_camera_restart_in_progress", False):
                return
            self._camera_restart_in_progress = True
            restart_success, restart_msg, data = self.command_executor.muto_interface.start_camera(check_before_start=True, wait_time_s=3.0)
            self.component_health['camera_system'] = bool(restart_success)
            try:
                node_running = data.get('node_running') if isinstance(data, dict) else None
                topic_active = data.get('topic_active') if isinstance(data, dict) else None
            except Exception:
                node_running = None
                topic_active = None
            self.get_logger().debug(f"Camera restart attempted: success={restart_success}, node_running={node_running}, topic_active={topic_active}")
        except Exception as e:
            self.get_logger().debug(f"Camera monitor error: {e}")
        finally:
            self._camera_restart_in_progress = False
    
    def _handle_error(self, error, context='unknown'):
        """统一的错误处理方法"""
        import time
        current_time = time.time()
        
        self.error_count += 1
        self.last_error_time = current_time
        
        if hasattr(self, 'get_logger'):
            self.get_logger().warn(f'Error in {context} (#{self.error_count}): {error}')
        else:
            print(f'Warning: Error in {context} (#{self.error_count}): {error}')
    
    def start_fastapi_server(self):
        """启动FastAPI服务器"""
        try:
            if self.app is None:
                self.get_logger().error('FastAPI application not available, cannot start server')
                return False
                
            self.get_logger().info(f'Starting FastAPI server on {self.server_host}:{self.server_port}')
            
            config = uvicorn.Config(
                app=self.app,
                host=self.server_host,
                port=self.server_port,
                log_level="info"
            )
            server = uvicorn.Server(config)
            
            # 在新线程中运行服务器
            def run_server():
                try:
                    asyncio.run(server.serve())
                except Exception as e:
                    if hasattr(self, 'get_logger'):
                        self.get_logger().error(f'FastAPI server error: {e}')
                    else:
                        print(f'FastAPI server error: {e}')
            
            server_thread = threading.Thread(target=run_server, daemon=True)
            server_thread.start()
            
            self.get_logger().info('FastAPI server started successfully')
            return True
            
        except Exception as e:
            self.get_logger().error(f'Failed to start FastAPI server: {e}')
            return False


def main(args=None):
    """主函数（单次启动，异常退出由launch respawn负责）"""
    try:
        # 初始化ROS2上下文
        if not rclpy.ok():
            rclpy.init(args=args)

        # 创建节点并启动HTTP服务
        node = MutoControllerNode()
        node.start_fastapi_server()

        # 进入ROS2事件循环
        rclpy.spin(node)

    except KeyboardInterrupt:
        print('\nShutting down Muto Controller Node...')
    except Exception as e:
        # 让异常向上暴露给launch层，交由respawn机制重启
        print(f'Fatal error in Muto Controller Node: {e}')
    finally:
        # 确保资源释放
        try:
            if 'node' in locals():
                node.destroy_node()
            if rclpy.ok():
                rclpy.shutdown()
        except Exception as shutdown_error:
            print(f'Error during ROS2 shutdown: {shutdown_error}')

    print('Muto Controller Node shutdown complete.')


if __name__ == '__main__':
    main()
